import time
import yaml
import cv2
import json
import numpy as np


#
# 解析 yaml 配置文件得到 resolution 分辨率和 origin 偏移量
#
with open('map.yaml', 'r') as file:
    yamldata = yaml.safe_load(file)
resolution = yamldata.get('resolution')
origin = yamldata.get('origin')
print("resolution:", resolution)
print("origin[0] :", origin[0])
print("origin[1] :", origin[1])


#
# 加载 pgm 图片，并解析得到高度信息
#
image_path = 'map.pgm'
gray = cv2.imread(image_path, cv2.IMREAD_GRAYSCALE)
height, width = gray.shape
print("width     :", width)
print("height    :", height)


#
# 解析 node txt 文件
#
def parse_txt_file(file_path):
    result = []
    try:
        with open(file_path, 'r') as file:
            for line in file:
                columns = line.strip().split()
                if len(columns) >= 4:
                    selected_columns = columns[1:4]
                    result.append(selected_columns)
    except FileNotFoundError:
        print(f"Error: File {file_path} not found.")
    except Exception as e:
        print(f"An unknown error occurred: {e}")
    return result

file_path = 'Make_Graph_node.txt'
data = parse_txt_file(file_path)

#
# 转化 3d 坐标到 2d 坐标
#
# 函数定义，3d 坐标转 2d 坐标，输出 2d 像素坐标
def dot_pix_coord(pcdx, pcdy, pcda):
    x = int((pcdx - origin[0]) / resolution)
    y = height - int((pcdy - origin[1]) / resolution)
    a = 0 - pcda
    return x, y, a


bak = cv2.merge([gray, gray, gray])
track_thickness = 3
track_color = (0, 0, 255)
points = []
for row in data:
    x, y, a = dot_pix_coord(float(row[0]), float(row[1]), float(row[2]))
    points.append((int(x), int(y)))
    print("x:", x, "y:", y, "a:", a)

# 固定箭头的像素大小
fixed_arrow_size = 40  # 可以根据需要调整


# 连接所有点绘制线段
for i in range(len(points) - 1):
    px1, py1 = points[i]
    px2, py2 = points[i + 1]
    # 计算线段长度
    line_length = np.sqrt((px2 - px1) ** 2 + (py2 - py1) ** 2)
    # 动态计算 tipLength
    tipLength = fixed_arrow_size / line_length if line_length > 0 else 0.1
    cv2.arrowedLine(bak, (px1, py1), (px2, py2), track_color, track_thickness, tipLength=tipLength)


# 连接最后一个点和第一个点，形成封闭图形
if len(points) > 0:
    px1, py1 = points[-1]
    px2, py2 = points[0]
    # 计算线段长度
    line_length = np.sqrt((px2 - px1) ** 2 + (py2 - py1) ** 2)
    # 动态计算 tipLength
    tipLength = fixed_arrow_size / line_length if line_length > 0 else 0.1
    cv2.arrowedLine(bak, (px1, py1), (px2, py2), track_color, track_thickness, tipLength=tipLength)


img = bak.copy()
# 原图尺寸太大，这里缩放下显示
resize_img = cv2.resize(img, ((int)(width / 2), (int)(height / 2)), interpolation=cv2.INTER_LINEAR)
cv2.imshow('PGM Image', resize_img)

cv2.waitKey(0)
cv2.destroyAllWindows()